
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <assert.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/mman.h>

//#include "zoom_lens.h"
#include "msg_func.h"
#include "cfg.h"

#include "fw/comm/inc/serial.h"
#include "mod/bsp/inc/bsp.h"
#include "mod/codec/inc/codec.h"
#include "mod/svp/inc/svp.h"
#include "spi_ms41929.h"
//#include "gpio.h"

static const char dev[] = "/dev/mem";

#define PAGE_SIZE_MASK (~(0xfff))
#define PAGE_SIZE 0x1000
#define PROT_READ 0x1             /* Page can be read.  */
#define PROT_WRITE 0x2            /* Page can be written.  */
#define PROT_EXEC 0x4             /* Page can be executed.  */
#define PROT_NONE 0x0             /* Page can not be accessed.  */
#define PROT_GROWSDOWN 0x01000000 /* Extend change to start of \
                     growsdown vma (mprotect only).  */
#define PROT_GROWSUP 0x02000000   /* Extend change to start of \
                     growsup vma (mprotect only).  */

/* Sharing types (must choose one and only one of these).  */
#define MAP_SHARED 0x01 /* Share changes.  */
// #define MAP_FAILED (NULL)

unsigned short highAndLowShiftHEX(unsigned short data)
{
    // printf("  before shift data=%x \n",data);

    data = (data << 8) | (data >> 8);                       // 0101101110010110
    data = ((data << 4) & 0xF0FF) | ((data >> 4) & 0xFF0F); // 1011010101101001
    data = ((data << 2) & 0xCCCC) | ((data >> 2) & 0x3333); // 1110010110010110
    data = ((data << 1) & 0xAAAA) | ((data >> 1) & 0x5555); // 1101101001101001
    // printf("  after shift data=%x \n",data);

    return data;
}

static void write_reg(unsigned int Addr, unsigned int Value)
{

    int fd = open(dev, O_RDWR | O_SYNC);
    if (fd < 0)
    {
        printf("open %s error!\n", dev);
        return;
    }

    /* addr align in page_size(4K) */
    unsigned long phy_addr_in_page;
    unsigned long page_diff;

    phy_addr_in_page = Addr & PAGE_SIZE_MASK; // #define PAGE_SIZE_MASK (~(0xfff))
    page_diff = Addr - phy_addr_in_page;

    /* size in page_size */
    unsigned long size_in_page;
    unsigned long size = PAGE_SIZE;
    size_in_page = ((size + page_diff - 1) & PAGE_SIZE_MASK) + PAGE_SIZE;

    void *addr = mmap((void *)0, size_in_page, PROT_READ | PROT_WRITE, MAP_SHARED, fd, phy_addr_in_page);
    if (addr == MAP_FAILED)
    {
        printf("mmap @ 0x%x error!\n", phy_addr_in_page);
        close(fd);
        return;
    }

    unsigned int *addr_cur = (unsigned int *)(addr + page_diff);
    *addr_cur = Value;
    /* munmap */
    if (munmap(addr, size_in_page) != 0)
    {
        printf("munmap failed!\n");
    }
    close(fd);
}

static void write_data(unsigned int Addr, unsigned int Value)
{
    int fd = open(dev, O_RDWR | O_SYNC);
    if (fd < 0)
    {
        printf("open %s error!\n", dev);
        return;
    }

    /* addr align in page_size(4K) */
    unsigned long phy_addr_in_page;
    unsigned long page_diff;

    phy_addr_in_page = Addr & PAGE_SIZE_MASK; // #define PAGE_SIZE_MASK (~(0xfff))
    page_diff = Addr - phy_addr_in_page;

    /* size in page_size */
    unsigned long size_in_page;
    unsigned long size = PAGE_SIZE;
    size_in_page = ((size + page_diff - 1) & PAGE_SIZE_MASK) + PAGE_SIZE;

    void *addr = mmap((void *)0, size_in_page, PROT_READ | PROT_WRITE, MAP_SHARED, fd, phy_addr_in_page);
    if (addr == MAP_FAILED)
    {
        printf("mmap @ 0x%x error!\n", phy_addr_in_page);
        close(fd);
        return;
    }

    unsigned int *addr_cur = (unsigned int *)(addr + page_diff);

    *addr_cur = Value >> 16;
    *addr_cur = Value;
    *addr_cur = Value >> 8;

    /* munmap */
    if (munmap(addr, size_in_page) != 0)
    {
        printf("munmap failed!\n");
    }
    close(fd);
}

static unsigned int read_reg(unsigned int Addr)
{
    unsigned int Value = 0;
    int fd = open(dev, O_RDWR | O_SYNC);
    if (fd < 0)
    {
        printf("open %s error!\n", dev);
        return -1;
    }

    /* addr align in page_size(4K) */
    unsigned long phy_addr_in_page;
    unsigned long page_diff;
    phy_addr_in_page = Addr & PAGE_SIZE_MASK;
    page_diff = Addr - phy_addr_in_page;

    /* size in page_size */
    unsigned long size_in_page;
    unsigned long size = PAGE_SIZE;
    size_in_page = ((size + page_diff - 1) & PAGE_SIZE_MASK) + PAGE_SIZE;

    void *addr = mmap((void *)0, size_in_page, PROT_READ | PROT_WRITE, MAP_SHARED, fd, phy_addr_in_page);
    if (addr == MAP_FAILED)
    {
        printf("mmap @ 0x%x error!\n", phy_addr_in_page);
        close(fd);
        return -1;
    }

    unsigned int *addr_cur = (unsigned int *)(addr + page_diff);

    Value = *addr_cur;

    /* munmap */
    if (munmap(addr, size_in_page) != 0)
    {
        printf("munmap failed!\n");
    }

    close(fd);
    // printf("ssp_read_reg %x %d\n", Addr, Value);
    return Value;
}

static void spi_enable(void)
{
    write_reg(SSP_CR1, 0x12); // 0x42);
}

static void spi_disable(void)
{
    write_reg(SSP_CR1, 0x0); // 0x40);
}

// void hi_ssp_enable(void)
// {
//     int ret = 0;
//     read_reg(SSP_CR1,ret);
//     ret = (ret & 0xFFFD) | 0x2;

//     ret = ret | (0x1 << 4); /* big/little end, 1: little, 0: big */
//     ret = ret | (0x1 << 15); /* wait en */
//     write_reg(SSP_CR1,ret);
// }
// void hi_ssp_disable(void)
// {
//     int ret = 0;
//     read_reg(SSP_CR1,ret);
//     ret = ret & (~(0x1 << 1));
//     write_reg(SSP_CR1,ret);
// }

int hi_spi_set_frameform(unsigned char framemode, unsigned char spo, unsigned char sph, unsigned char datawidth, unsigned char bitend)
{
    int ret = 0;
    ret = read_reg(SSP_CR0);
    if (framemode > 3)
    {
        printf("[ERROR]set frame parameter err.\n");
        return -1;
    }
    ret = (ret & 0xFFCF) | (framemode << 4);
    if ((ret & 0x30) == 0)
    {
        if (spo > 1)
        {
            printf("[ERROR]set spo parameter err.\n");
            return -1;
        }
        if (sph > 1)
        {
            printf("[ERROR]set sph parameter err.\n");
            return -1;
        }
        ret = (ret & 0xFF3F) | (sph << 7) | (spo << 6);
    }
    if ((datawidth > 16) || (datawidth < 4))
    {
        printf("[ERROR]set datawidth parameter err.\n");
        return -1;
    }
    ret = (ret & 0xFFF0) | (datawidth - 1);
    write_reg(SSP_CR0, ret);

    ret = read_reg(SSP_CR1);
    if (bitend == 1)
    {
        ret = ret | (0x10);
        write_reg(SSP_CR1, ret);
    }
    ret = read_reg(SSP_CR1);
    printf("SSP_CR1:0x%x\n", ret);
    return 0;
}

int hi_spi_set_serialclock(unsigned char scr, unsigned char cpsdvsr)
{
    int ret = 0;
    ret = read_reg(SSP_CR0);
    ret = (ret & 0xFF) | (scr << 8);
    write_reg(SSP_CR0, ret);
    if ((cpsdvsr & 0x1))
    {
        printf("[ERROR]set cpsdvsr parameter err.\n");
        return -1;
    }
    write_reg(SSP_CPSR, cpsdvsr);
    return 0;
}

int hi_spi_alt_mode_set(int enable)
{
    int ret = 0;

    ret = read_reg(SSP_CR1);
    if (enable)
    {
        ret = ret & (~0x40);
    }
    else
    {
        ret = (ret & 0xFF) | 0x40;
    }
    write_reg(SSP_CR1, ret);

    return 0;
}

static int hi_spi_check_timeout(void)
{
    unsigned int value = 0;
    unsigned int tmp = 0;
    while (1)
    {
        value = read_reg(SSP_SR);
        if ((value & SPI_SR_TFE) && (!(value & SPI_SR_BSY)))
        {
            break;
        }

        if (tmp++ > MAX_WAIT)
        {
            printf("spi transfer wait timeout!\n");
            return -1;
        }
        usleep(1);
    }
    return 0;
}

unsigned int spi_ms41929_write(unsigned char addr, unsigned short data)
{
    int ret = 0;
    unsigned int spicr0_value = 0;
    unsigned int receive_data = 0;
    unsigned int send_data = 0;

    // unsigned int return_data=0;
    // while (1) // clear recive data fifo
    // {
    //     spicr0_value = read_reg(SSP_SR);
    //     if (spicr0_value & SPI_SR_RNE) // fifo is not empty
    //     {
    //         receive_data = read_reg(SSP_DR);
    //     }
    //     else
    //     {
    //         break;
    //     }
    // }

    send_data = addr; //<<2;
    send_data = send_data << 16;
    send_data += data;

    // printf("send_data:0x%0x\n",send_data);

    spi_enable();
    write_data(SSP_DR, send_data);

    ret = hi_spi_check_timeout();
    if (ret != 0)
    {
        printf("spi_send timeout\n");
    }
    spi_disable();
    // return_data=recive_data;
    return receive_data;
}

unsigned short spi_ms41929_read(unsigned char addr)
{

    int ret = 0;
    unsigned int spicr0_value = 0;
    unsigned short receive_data = 0;
    unsigned int send_data = 0;
    // unsigned int return_data=0;
    while (1) // clear recive data fifo
    {
        spicr0_value = read_reg(SSP_SR);
        if (spicr0_value & SPI_SR_RNE) // fifo is not empty
        {
            read_reg(SSP_DR);
        }
        else
        {
            break;
        }
    }

    send_data = addr | 0x40; //<<2;
    // send_data+=2;
    send_data = send_data << 16;

    spi_enable();
    write_data(SSP_DR, send_data);

    ret = hi_spi_check_timeout();
    if (ret != 0)
    {
        printf("spi_send timeout\n");
    }
    spi_disable();
    usleep(10); // how long is suitable?
    // write_reg(SSP_DR, addr);
    while (1) // clear recive data fifo
    {
        spicr0_value = read_reg(SSP_SR);
        printf("cr0:0x%0x\n", spicr0_value);
        if (spicr0_value & SPI_SR_RNE)
        {
            receive_data = receive_data << 8;
            receive_data = receive_data + read_reg(SSP_DR);
            // break;  //how about having more than one data? and receiving none,it will loop forever?
        }
        else
        {
            break;
        }
    }
    receive_data = highAndLowShiftHEX(receive_data);
    return receive_data;
}

void spi_ms41929_init()
{
    spi1_pinmux();
    spi1_open_clock();
    spi_disable();
    // hi_spi_set_frameform(SPI_FRAMEMODE_MOTOR, SPI_SPO, SPI_SPH, SPI_DATA_WIDTH_8);
    hi_spi_set_serialclock(SPI_SCR, SPI_CPSDVSR);
    hi_spi_alt_mode_set(1);
    // spi_ms41929_VDFZ_init();
}

void spi_ms41929_write_set_framemode()
{
    // spi1_pinmux();
    // spi1_open_clock();
    // spi_disable();
    hi_spi_set_frameform(SPI_FRAMEMODE_MOTOR, SPI_SPO, SPI_SPH, SPI_DATA_WIDTH_8, 1);
    // hi_spi_set_serialclock(SPI_SCR, SPI_CPSDVSR);
    // hi_spi_alt_mode_set(1);
}

void spi_ms41929_read_init()
{
    spi1_pinmux();
    spi1_open_clock();
    spi_disable();

    hi_spi_set_serialclock(SPI_SCR, SPI_CPSDVSR);
    hi_spi_alt_mode_set(1);
    hi_spi_set_frameform(SPI_FRAMEMODE_NATION, SPI_SPO, SPI_SPH, SPI_DATA_WIDTH_16, 1); // national model,16 bits
}

void spi1_csn0_pinmux()
{
    // 复选spi_csn0
    write_reg(SPI1_CSN0, 0x0501);

    // 配置csn0低有效 且选择片选0
    unsigned int misc_ctrl0;
    misc_ctrl0 = read_reg(MISC_CTRL0);
    misc_ctrl0 |= (0x1 << 3); // high enable
    // misc_ctrl0 &= ~(0x1<<3); // low enable
    misc_ctrl0 &= ~(0x1 << 2); // 片选0
    write_reg(MISC_CTRL0, misc_ctrl0);
}
void spi1_pinmux()
{    
    write_reg(SPI1_SCLK, 0x0631);
    write_reg(SPI1_SDO, 0x0631);
    write_reg(SPI1_SDI, 0x01);

    spi1_csn0_pinmux();

    printf("spi1_pinmux\n");
}

void spi1_open_clock()
{
    unsigned int peri_cfg;
    peri_cfg = read_reg(PERI_CRG111);
    write_reg(PERI_CRG111, peri_cfg | 0x1 << 13);
    printf("spi1_open_clock\n");
}

void pwm0_init()
{
    unsigned int sr_value = 0;
    sr_value = read_reg(PERI_CRG111);
    // set [9:8]=00 select clock 3MHz,set [7]=1,open clock
    sr_value = sr_value & (~(0x11 << 8));
    sr_value = sr_value | (0x1 << 7);
    write_reg(PERI_CRG111, sr_value); // pin mux select pwm0

    write_reg(PWM0_PIN6_6_IO_CONF, 0x0531); // pin mux select pwm0

    sr_value = read_reg(PWM0_STATE2);
    sr_value = sr_value & (0x1 << 10); // PWM0_STATE2 [10] ==1 busy, 0 idel
    if (sr_value == 0)
    {
        // idle
    }

    // period ,clocks : 3M/100Hz=30000=0x7530
    write_reg(PWM0_CFG0, 0x7530); //

    // duty ratio %50,high plus clocks:  0xea60/2=0x3a98
    write_reg(PWM0_CFG1, 0x3a98);

    // plus number
    //  write_reg(PWM0_CFG2, 0xaa);

    // enable and keep
    // write_reg(PWM0_CTRL, 0x5);
}

void pwm0_enable()
{
    // enable and keep
    // pwm0_init();
    write_reg(PWM0_CTRL, 0x5);
}

void pwm0_disable()
{
    // enable and keep
    write_reg(PWM0_CTRL, 0x0);
}

// void spi_ms41929_VDFZ_init()
// {
//     // write_reg(0x111f0024, 0x0600);//gpio6_6

//     // write_reg(0x120d6400,0x40);
//     PINMUX_REG_S pinmux[1] = {0x111f0024, 0x0600};
//     HI_GPIOMNG_Init(pinmux, 1);
//     HI_GPIOMNG_SetDir(6 * 8 + 6, 1);
// }

// void spi_ms41929_VDFZ_pulse()
// {
//     // write_reg(0x120d6100,0x40);
//     HI_GPIOMNG_SetVal(6 * 8 + 6, 1);
//     HI_GPIOMNG_SetVal(6 * 8 + 6, 0);
// }
// void spi_ms41929_VDFZ_low()
// {
//     // write_reg(0x120d6100,0x0);
//     HI_GPIOMNG_SetVal(6 * 8 + 6, 0);
// }

// int (*gsf_lens_zoom)(int ch, int dir, int speed)
// {
//     printf("zoom,ch:%d,dir:%d,speed:%d\n",ch,dir,speed);
//     if (dir == 0)
//     {
//         spi_ms41929_write(0x24, 0x045f);
//     }
//     else
//     {
//         spi_ms41929_write(0x24, 0x055f);
//     }
//     spi_ms41929_write(0x25, 0x05ff * speed);

//     pwm0_enable();

//     return 1;
// }

// int (*gsf_lens_focus)(int ch, int dir, int speed)
// {
//      printf("focus,ch:%d,dir:%d,speed:%d\n",ch,dir,speed);
//     if (dir == 0)
//     {
//         spi_ms41929_write(0x29, 0x045f);
//     }
//     else
//     {
//         spi_ms41929_write(0x29, 0x055f);
//     }
//     spi_ms41929_write(0x2a, 0x05ff * speed);

//     pwm0_enable();

//     return 1;
// }

// int (*gsf_lens_stop)(int ch)
// {
//     pwm0_disable();
// };

int zoom_lens_init()
{
     /*How to set the time ?
     1.about 120us during sending a data through spi
     2.at last sending  4 datas every controling
     3.so set DT1 about 606us  
     4.set DT2 2,606us
     */
    spi_ms41929_init();
    spi_ms41929_write_set_framemode();
   
    //dt1 , 20H[7:0]  , 1 stand for 303.us ,set to 2,606us
    //PWMRES:0,PWIMMODE:10,CLOCK:337.5Khz
    spi_ms41929_write(0x20, 0x0a02);

   //DT2A:2
    spi_ms41929_write(0x22, 0x0002);
    //PPWB:0x28,30  PPWA:0x28,30  
    spi_ms41929_write(0x23, 0x2828);
    //spi_ms41929_write(0x24, 0x04ff);
    //spi_ms41929_write(0x25, 0x1ab5);
    //DT2A:2
    spi_ms41929_write(0x27, 0x0002);
    //PPWC:0x28,30  PPWD:0x28,30  
    spi_ms41929_write(0x28, 0x2828);
    //spi_ms41929_write(0x29, 0x04ff);
    //spi_ms41929_write(0x2a, 0x1ab5);
    spi_ms41929_write(0x0b, 0x80);
    spi_ms41929_write(0x21, 0x0085);

    pwm0_init();
    printf("zoom_lens_int ok\n");
    return 0;
}